#include "main.h"
#include "motor_car.h"
#include "line_pid.h"
#include "Ultrasonic.h"

// 全局变量
int track_flag = 0;
char send_buf[35] = {0};
uint8_t rx_buff[Package_size];
uint8_t new_package[Package_size];
uint8_t g_new_package_flag = 0;

// 网络和MQTT配置
const int Press_KEY = 0;
const int Release_KEY = 1;
const char *mqtt_broker = "mixio.mixly.cn";
const char *mqtt_username = "1106727349@qq.com";
const char *mqtt_password = "535e12041f5618f6dc0a5a0543ea4328";
const int mqtt_port = 1883;

String mqtt_topic = "";
String mqtt_data = "";
boolean mqtt_status = false;
String project = "qny-test";
WiFiClient espClient;
PubSubClient client(espClient);

// 创建超声波对象
Ultrasonic ultrasonic(5, 18);

void callback(char *topic, byte *payload, unsigned int length) {
    String data = "";
    for (int i = 0; i < length; i++) {
        data = String(data) + String((char)payload[i]);
    }
    mqtt_topic = String(topic);
    mqtt_data = data;
    mqtt_status = true;
}

void uploadData(const char *subTopic, const char *payload) {
    char topic[128];
    snprintf(topic, sizeof(topic), "%s/%s/%s", mqtt_username, project, subTopic);
    if (client.publish(topic, payload)) {
        // 发布成功
    } else {
        // 发布失败
    }
}

void setup() {
    Serial.begin(115200);
    Motor_init();

    // WiFi连接
    WiFi.begin("雷军我要开小米SU7", "20031104");
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("Local IP:");
    Serial.print(WiFi.localIP());
    Serial.println(WiFi.macAddress());
    Serial.println("wifi已连接");
    delay(1000);

    // MQTT设置
    client.setServer(mqtt_broker, mqtt_port);
    client.setCallback(callback);

    while (!client.connected()) {
        String client_id = "esp-client-";
        client_id += String(WiFi.macAddress());
        Serial.printf("The client %s connects to the public mqtt broker\n", client_id.c_str());
        if (client.connect(client_id.c_str(), mqtt_username, mqtt_password)) {
            Serial.println("Public emqx mqtt broker connected");
            client.publish(String(String(mqtt_username) +"/"+ String(project) +"/"+ String("b640a0ce465fa2a4150c36b305c1c11b")).c_str(),String(client_id).c_str());
        } else {
            Serial.print("failed with state ");
            Serial.print(client.state());
            delay(2000);
        }
    }

    String topicButton = String(mqtt_username) + "/" + String(project) + "/button";
    client.subscribe(topicButton.c_str());

    init_x_PID(); // PID初始化
    Wire.begin(); // I2C初始化
    delay(100);
}

void loop() {
    client.loop();

    // 检查障碍物
    if (ultrasonic.checkObstacle(20.0)) {
        // 遇到障碍物，执行避障动作
        setCarMove(STOP, 0);
        delay(200);
        setCarMove(BACKWARD, 150);
        delay(500);
        setCarMove(STOP, 0);
        delay(200);

        // 随机转向避开障碍物
        if (random(0, 2) == 0) {
            setCarMove(LEFT, 150);
        } else {
            setCarMove(RIGHT, 150);
        }
        delay(600);
        setCarMove(STOP, 0);
        delay(200);

        uploadData("status", "OBSTACLE_AVOIDED");
    } else {
        // 无障碍物，正常循迹
        Car_line_track();
    }

    delay(50);
    I2Cdata();

    if (mqtt_status) {
        if (String(mqtt_topic).equals(String(String(mqtt_username) +"/"+ String(project) +"/"+ String("button")))) {
            Serial.println(mqtt_data);
            if (mqtt_data == "1") {
                track_flag = 1;
            } else {
                track_flag = 0;
                setCarMove(STOP, 0); 
            }
            mqtt_status = false;
        }
    }
}

void I2Cdata() {
    byte data = 0;
    char bufbuf[50] = {'\0'};
    Wire.beginTransmission(0x12);
    Wire.write(0x30);
    Wire.endTransmission();

    delay(10);

    Wire.requestFrom(0x12, 1);
    while (Wire.available()) {
        data = Wire.read();
    }

    x1 = (data>>7)&0x01;
    x2 = (data>>6)&0x01;
    x3 = (data>>5)&0x01;
    x4 = (data>>4)&0x01;
    x5 = (data>>3)&0x01;
    x6 = (data>>2)&0x01;
    x7 = (data>>1)&0x01;
    x8 = (data>>0)&0x01;

    sprintf(bufbuf,"x1:%d,x2:%d,x3:%d,x4:%d,x5:%d,x6:%d,x7:%d,x8:%d\r\n",x1,x2,x3,x4,x5,x6,x7,x8);
}